#include "drone_pkg/Control.h"
#include <cmath>
#include "Control.h"
#include <stdlib.h>


Control::Control(double MaxLinerSpeed = 20,
                      double MaxAngularSpeed = 15,
                      double KPLP = 0.5,
                      double KPLD = 0.02,
                      double KPA  = 0,
                      double MinXYP = 4,
                      double MinXYD = 20,
                      double MinYaw = 50
                      ) : MAX_LINEAR_SPEED(MaxLinerSpeed),
                          MAX_ANGULAR_SPEED(MaxAngularSpeed),
                          KP_LINEAR_SPEED_POINT(KPLP),
                          KP_LINEAR_SPEED_DETECT(KPLD),
                          KP_ANGULAR_SPEED(KPA),
                          MIN_ERROR_XY_POINT(MinXYP),
                          MIN_ERROR_XY_DETECT(MinXYD),
                          MIN_ERROR_YAW(MinYaw)

{
    /*初始化变量*/

    /*目标变量*/
    targetPointX = 0;
    targetPointY = 0;
    targetDetectX = 320;
    targetDetectY = 320;
    targetYaw = 0;
    targetNum = 8;
    
    /*中间变量*/
    distanceXY = 0;
    errorPointX = 0;
    errorPointY = 0;
    errorDetectX = 0;
    errorDetectY = 0;
    actualPositionX = 0;
    actualPositionY = 0;
    actualPositionYaw = 0;
    detectX = 0;
    detectY = 0;
    errorYaw = 0;
    cosTheta = 0;
    sinTheta = 0;
    startTime = 0;
    presentTime = 0;
    deltaTime = 0;

    /*结果变量*/
    resultLinearSpeedXY = 0;
    resultLinearSpeedX = 0;
    resultLinearSpeedY = 0;
    resultAngularSpeedYaw = 0;

    /*结果变量*/
    flagMovePoint = 0;
    flagDetect = 0;
    mode = 0;
    detectNum = -1;
    flagLaser = 0;
}






